{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## PurePursuit实现轨迹跟踪实现轨迹跟踪\n",
    "参考[博客](https://blog.csdn.net/weixin_42301220/article/details/124882144?spm=1001.2014.3001.5501)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "假设无人车模型如下\n",
    "\n",
    "![在这里插入图片描述](https://img-blog.csdnimg.cn/98de36e913bd4fcd86b4f3ac933b0afc.png)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib\n",
    "import matplotlib.pyplot as plt\n",
    "# %matplotlib inline\n",
    "# %matplotlib notebook\n",
    "%matplotlib qt5\n",
    "# %matplotlib auto\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "import math\n",
    "class KinematicModel_3:\n",
    "  \"\"\"假设控制量为转向角delta_f和加速度a\n",
    "  \"\"\"\n",
    "\n",
    "  def __init__(self, x, y, psi, v, L, dt):\n",
    "    self.x = x\n",
    "    self.y = y\n",
    "    self.psi = psi\n",
    "    self.v = v\n",
    "    self.L = L\n",
    "    # 实现是离散的模型\n",
    "    self.dt = dt\n",
    "\n",
    "  def update_state(self, a, delta_f):\n",
    "    self.x = self.x+self.v*math.cos(self.psi)*self.dt\n",
    "    self.y = self.y+self.v*math.sin(self.psi)*self.dt\n",
    "    self.psi = self.psi+self.v/self.L*math.tan(delta_f)*self.dt\n",
    "    self.v = self.v+a*self.dt\n",
    "\n",
    "  def get_state(self):\n",
    "    return self.x, self.y, self.psi, self.v\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "参数设置"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [],
   "source": [
    "L=2 # 车辆轴距，单位：m\n",
    "v = 2 # 初始速度\n",
    "x_0=0 # 初始x\n",
    "y_0=-3 #初始y\n",
    "psi_0=0 # 初始航向角\n",
    "dt=0.1 # 时间间隔，单位：s\n",
    "lam = 0.1 # 前视距离系数\n",
    "c=2 # 前视距离"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "搜索目标临近点"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [],
   "source": [
    "def cal_target_index(robot_state, refer_path,l_d):\n",
    "    \"\"\"得到前视目标点\n",
    "\n",
    "    Args:\n",
    "        robot_state (_type_): 当前车辆位置\n",
    "        refer_path (_type_): 参考轨迹（数组）\n",
    "        l_d：前视距离\n",
    "    Returns:\n",
    "        _type_: 前视目标点的索引\n",
    "    \"\"\"\n",
    "    dists = []\n",
    "    for xy in refer_path:\n",
    "        dis = np.linalg.norm(robot_state-xy)\n",
    "        dists.append(dis)\n",
    "\n",
    "    min_index = np.argmin(dists)\n",
    "    \n",
    "    delta_l = np.linalg.norm(refer_path[min_index]-robot_state)\n",
    "    # 搜索前视目标点\n",
    "    while l_d > delta_l and (min_index+1) < len(refer_path):\n",
    "        delta_l = np.linalg.norm(refer_path[min_index+1]-robot_state)\n",
    "        min_index += 1\n",
    "    return min_index\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "pure pursuit"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [],
   "source": [
    "def pure_pursuit_control(robot_state,current_ref_point,l_d):\n",
    "    \"\"\"pure pursuit\n",
    "\n",
    "    Args:\n",
    "        robot_state (_type_): 车辆位置\n",
    "        current_ref_point (_type_): 当前参考路点\n",
    "        l_d：前视距离\n",
    "    return:返回前轮转向角delta\n",
    "    \"\"\"\n",
    "    alpha = math.atan2(current_ref_point[1]-robot_state[1], current_ref_point[0]-robot_state[0])-ugv.psi\n",
    "    delta = math.atan2(2*L*np.sin(alpha),l_d)\n",
    "    return delta\n",
    "    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "主函数"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "MovieWriter ffmpeg unavailable; using Pillow instead.\n"
     ]
    }
   ],
   "source": [
    "from celluloid import Camera  # 保存动图时用，pip install celluloid\n",
    "\n",
    "\n",
    "# set reference trajectory\n",
    "refer_path = np.zeros((1000, 2))\n",
    "refer_path[:,0] = np.linspace(0, 100, 1000) # 直线\n",
    "refer_path[:,1] = 2*np.sin(refer_path[:,0]/3.0)+2.5*np.cos(refer_path[:,0]/2.0) # 生成正弦轨迹\n",
    "\n",
    "\n",
    "\n",
    "ugv = KinematicModel_3(x_0,y_0,psi_0,v,L,dt)\n",
    "\n",
    "x_ = []\n",
    "y_ = []\n",
    "fig = plt.figure(1)\n",
    "# 保存动图用\n",
    "camera = Camera(fig)\n",
    "for i in range(600):\n",
    "    robot_state = np.zeros(2)\n",
    "    robot_state[0] = ugv.x\n",
    "    robot_state[1] = ugv.y\n",
    "    \n",
    "    l_d = lam*ugv.v+c # 注意，这里的运动学模型使用的速度v就是车身纵向速度vx\n",
    "    ind = cal_target_index(robot_state,refer_path,l_d)  # 搜索前视路点\n",
    "\n",
    "\n",
    "    delta = pure_pursuit_control(robot_state,refer_path[ind],l_d)\n",
    "\n",
    "    ugv.update_state(0,delta) # 加速度设为0，恒速\n",
    "\n",
    "    x_.append(ugv.x)\n",
    "    y_.append(ugv.y)\n",
    "\n",
    "    # 显示动图\n",
    "    # plt.cla()\n",
    "    plt.plot(refer_path[:, 0], refer_path[:, 1], '-.b', linewidth=1.0)\n",
    "    plt.plot(x_, y_, \"-r\", label=\"trajectory\")\n",
    "    plt.plot(refer_path[ind, 0], refer_path[ind, 1], \"go\", label=\"target\")\n",
    "    # plt.axis(\"equal\")\n",
    "    plt.grid(True)\n",
    "    # plt.pause(0.001)\n",
    "    camera.snap()\n",
    "animation = camera.animate()\n",
    "animation.save('trajectory.gif')\n",
    "\n",
    "plt.figure(2)\n",
    "plt.plot(refer_path[:,0], refer_path[:,1], '-.b', linewidth=1.0)\n",
    "plt.plot(x_,y_,'r')\n",
    "plt.show()\n"
   ]
  }
 ],
 "metadata": {
  "interpreter": {
   "hash": "0c7484b3574347463e16b31029466871583b0d4e5c4ad861e8848f2d3746b4de"
  },
  "kernelspec": {
   "display_name": "Python 3.8.12 ('gobigger')",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.12"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
